import rclpy
from rclpy.node import Node

import threading
from topic_msg.msg import CatSpeak
from topic_msg.msg import CatDeriction
import time
from mpu6050 import mpu6050
import smbus


class Gyroscope:
    VERTICAL = 0  #
    HORIZONTAL = 1  #
    MPU6050_ADR = 0x68

    def __init__(self):
        MPU6050_ADR = 0x68
        self._sensor = mpu6050(Gyroscope.MPU6050_ADR)  # I2C地址
        self.accel_x = 0
        self.accel_y = 0
        self.accel_z = 0
        self.gyro_x = 0
        self.gyro_y = 0
        self.gyro_z = 0
        self.temp = 0

    def _get_data(self):
        time.sleep(0.1)
        accel = self._sensor.get_accel_data()
        time.sleep(0.1)
        gyro = self._sensor.get_gyro_data()
        time.sleep(0.1)
        temp = self._sensor.get_temp()
        time.sleep(0.1)

        self.accel_x = accel["x"]
        self.accel_y = accel["y"]
        self.accel_z = accel["z"]

        self.gyro_x = gyro["x"]
        self.gyro_y = gyro["y"]
        self.gyro_z = gyro["z"]

        self.temp = temp

        # print(f"Accel: X:{accel['x']:.2f}g Y:{accel['y']:.2f}g Z:{accel['z']:.2f}g")
        # print(f"Gyro : X:{gyro['x']:.2f}°/s Y:{gyro['y']:.2f}°/s Z:{gyro['z']:.2f}°/s")
        # print(f"Temp : {temp:.1f}°C\n")

    def get_status(self):
        self._get_data()
        if self.accel_z < 9:
            return Gyroscope.HORIZONTAL
        else:
            return Gyroscope.VERTICAL


class Power:
    ADDR = 0x2D
    BATTERY_STATISTICS = 0x20
    BATTERY_STATISTICS_LEN = 0x0C
    BATTERY_CELL_STATISTICS = 0x30
    BATTERY_CELL_STATISTICS_LEN = 0x08
    LOW_VOLTAGE = 3150  # mV
    LOW_CURRENT = 50  # mA
    HIGH_POWER = 0
    LOW_POWER = 1

    def __init__(self):
        self._bus = smbus.SMBus(1)
        self.current = 0  # cat power
        self.cell_vol_1 = 0  # cat power
        self.cell_vol_2 = 0  # cat power
        self.cell_vol_3 = 0  # cat power
        self.cell_vol_4 = 0  # cat power

    def _get_data(self):
        time.sleep(0.1)
        self._get_battery_statistic()
        time.sleep(0.1)
        self._get_battery_cell_voltage()
        time.sleep(0.1)

        # print(f"current:{self.current}")
        # print(f"cell_vol_1:{self.cell_vol_1}")
        # print(f"cell_vol_2:{self.cell_vol_2}")
        # print(f"cell_vol_3:{self.cell_vol_3}")
        # print(f"cell_vol_4:{self.cell_vol_4}")

    def _get_battery_statistic(self):
        data = self._bus.read_i2c_block_data(
            Power.ADDR, Power.BATTERY_STATISTICS, Power.BATTERY_STATISTICS_LEN
        )
        self.current = data[2] | data[3] << 8
        if self.current > 0x7FFF:
            self.current -= 0xFFFF

    def _get_battery_cell_voltage(self):
        data = self._bus.read_i2c_block_data(
            Power.ADDR, Power.BATTERY_CELL_STATISTICS, Power.BATTERY_CELL_STATISTICS_LEN
        )
        self.cell_vol_1 = data[0] | data[1] << 8
        self.cell_vol_2 = data[2] | data[3] << 8
        self.cell_vol_3 = data[4] | data[5] << 8
        self.cell_vol_4 = data[6] | data[7] << 8

    def get_status(self):
        self._get_data()
        if self.current < Power.LOW_CURRENT and (
            self.cell_vol_1 < Power.LOW_CURRENT
            or self.cell_vol_2 < Power.LOW_CURRENT
            or self.cell_vol_3 < Power.LOW_CURRENT
            or self.cell_vol_4 < Power.LOW_CURRENT
        ):
            return Power.LOW_POWER
        else:
            return Power.HIGH_POWER


class CatIicNode(Node):
    def __init__(self, name):
        super().__init__(name)
        self._iic_lock = threading.Lock()
        self._gyroscope = Gyroscope()
        self._power = Power()

        self._speak_publisher = self.create_publisher(CatSpeak, "cat_speak", 10)

        self._deriction_publisher = self.create_publisher(
            CatDeriction, "cat_deriction", 10
        )

        self._thd = threading.Thread(target=self._run)
        self._thd.start()

    def __del__(self):
        self._thd.join()
        self._speak_publisher.destroy()
        self._deriction_publisher.destroy()

    def _run(self):
        emegency = 0
        while rclpy.ok():
            time.sleep(0.1)
            try:
                msg = CatDeriction()
                msg.deriction = self._gyroscope.get_status()
                self._deriction_publisher.publish(msg)

                if (
                    self._gyroscope.temp > 75
                    or self._power.get_status() == Power.LOW_POWER
                ):
                    if emegency == 3:
                        msg = CatSpeak()
                        msg.announce = CatSpeak.ERGENCY
                        self._speak_publisher.publish(msg)
                        emegency = 0
                    else:
                        emegency += 1
                else:
                    emegency = 0

            except Exception as e:
                self.get_logger().info(f"error: {e}")


def main():
    rclpy.init()
    node = CatIicNode("cat_iic")
    rclpy.spin(node)
    rclpy.shutdown()
